#include "ParametricCurves3.h"

#include "../Core/Exceptions.h"
#include "../Core/RealSquareMatrices.h"
#include "../Core/LinearCombination3.h"

namespace cagd {

    // homework: special constructor
    ParametricCurve3::ParametricCurve3(GLdouble u_min, GLdouble u_max, GLboolean is_closed, const ColumnMatrix<Derivative> &derivative):
            _u_min(u_min), _u_max(u_max),
            _is_closed(is_closed),
            _derivative(derivative)
    {
        bspc = 0;
    }

    ParametricCurve3::~ParametricCurve3() {
        if (bspc) {
            delete bspc, bspc = 0;
        }
    }

    // homework: calculate the energy at a given curve point
    GLdouble ParametricCurve3::Energy(GLdouble u, GLdouble w_l, GLdouble lambda, GLdouble w_c, GLdouble mu) const {
        return 0;
    }

    // homework: calculate the total energy along the curve
    GLdouble ParametricCurve3::TotalEnergy(GLdouble w_l, GLdouble lambda, GLdouble w_c, GLdouble mu) const {
        return 0;
    }

    // homework: evaluate the given order derivative at the parameter value u
    DCoordinate3 ParametricCurve3::operator ()(GLuint order, GLdouble u) const {
        return _derivative(order)(u);
    }

    // homework: using the user defined function pointers generate the image and derivatives of the given parametric curve
    GenericCurve3* ParametricCurve3::GenerateImage(GLuint div_point_count, GLenum usage_flag) const {
        GenericCurve3* result = 0;

        result = new GenericCurve3(_derivative.GetRowCount() - 1, div_point_count, usage_flag);

        if (!result)
            return 0;

        // set derivatives at the endpoints of the parametric curve
        for (GLuint order = 0; order < _derivative.GetRowCount(); ++order)
        {
            (*result)(order, 0) = _derivative(order)(_u_min);
            (*result)(order, div_point_count - 1) = _derivative(order)(_u_max);
        }

        // calculate derivatives at inner curve points
        GLdouble u_step = (_u_max - _u_min) / (div_point_count - 1);
        GLdouble u = _u_min;

        for (GLuint i = 1; i < div_point_count - 1; ++i)
        {
            u += u_step;

            for (GLuint order = 0; order < _derivative.GetRowCount(); ++order)
            {
                (*result)(order, i) = _derivative(order)(u);
            }
        }

        return result;
    }

    // homework:
    //
    // using the studied least squares algorithm and multi-threading (QtConcurrent::blockingMap),
    // approximate the shape of the given parametric curve with a B-spline curve
    //
    // data_usage_flag is
    //
    // if the parametric curve is periodic (open) then you must use a periodic (clamped) knot vector
    BSplineCurve3* ParametricCurve3::ConvertToBSplineCurve(
            GLuint p,                               // order of the B-spline curve
            GLuint n,                               // n + 1 denotes the number of unrepeated control points
            GLdouble w_l,                           // energy parameter
            GLdouble lambda,                        // energy parameter
            GLdouble w_c,                           // energy parameter
            GLdouble mu,                            // energy parameter
            GLuint integral_div_points,             // subdivision points needed by the Simpson quadrature formula
            GLdouble &error,                        // denotes the distance between the given parametric curve and the approximating B-spline curve
            GLenum data_usage_flag                  //an input parameter of the BSplineCurve3's special constructor
            )
    {
        if (_is_closed) {
            bspc = new BSplineCurve3(KnotVector::PERIODIC, p, n, _u_min, _u_max, data_usage_flag);
        } else {
            bspc = new BSplineCurve3(KnotVector::CLAMPED, p, n, _u_min, _u_max, data_usage_flag);
        }

////        A = @(u,n,i) nchoosek(n, i) * u.^i .* (1-u).^(n-i);%[0,1]
////        RowMatrix<GLdouble> ta = TA();

//        RowMatrix<GLdouble> u;
//        u.ResizeColumns(integral_div_points);

//        GLdouble step = (_u_max - _u_min) / integral_div_points;

//        for (GLint i =0; i< integral_div_points; i++)
//        {
//            u[i] = _u_min + i * step;
//        }

//        RealSquareMatrix alpha;
//        for (GLint i = 0; i<n; i++)
//        {
//            for (GLint j=0; j<n-i; j++)
//            {
////               RowMatrix<GLdouble> taProd = prodRowMatrix(TA(u,n,i), TA(u,n,j));
////               alpha(i+1, j+1) = f()
//            }
//        }
        RealSquareMatrix alpha;
        alpha.ResizeRows(n+1);
        for (GLuint i = 0; i <= n; i++) {
            for (GLuint j = 0; j <= n-i; j++) {
                alpha(i, j) = Length(integral_div_points);           // Integral
                alpha(n-j, n-i) = alpha(i, j);
            }
        }

        Matrix<GLdouble> b;
        b.ResizeRows(2);
        b.ResizeColumns(n+1);

        for (GLuint i = 0; i <=n; i++) {
                                                    // f_1 = @(v)TA(v, n, i) .* c_1(v);
            b(0,i) = Length(integral_div_points);   // b(1,i+1) = quad(f_1, u_min, u_max);
                                                    // f_2 = @(v)TA(v, n, i) .* c_2(v);
            b(1,i) = Length(integral_div_points);   // b(2,i+1) = quad(f_2, u_min, u_max);
        }

        ColumnMatrix<GLdouble> B;
        B.ResizeRows((n+1) * 2);

        for (GLuint i = 0; i <= n; i++) {
            B(2*i) = b(0, i);
            B(2*i) = b(1, i);
        }

        RealSquareMatrix M;
        M.ResizeRows((n+1) * 2);

        for (GLuint r = 0; r <= n; r++) {
            RowMatrix<GLdouble> x;
            x.ResizeColumns(alpha.GetColumnCount());

            for (GLuint i = 0; i < alpha.GetColumnCount(); i++) {
                x[i] = alpha(r, i);
                x[alpha.GetColumnCount()+i] = 0;
            }

            for (GLuint i = 0; i < M.GetColumnCount(); i++) {
                M(2 * r, i) = x[i];
                M(2 * r, M.GetColumnCount() + i) = 0;
                M(2 * r + 1, M.GetColumnCount() + i) = x[i];
                M(2 * r + 1, i) = 0;
            }
        }

        Matrix<GLdouble> A;
        if (!M.SolveLinearSystem(B,A)) {
            throw Exception("Error at solve Linear system!");
        }

    }

    GLdouble ParametricCurve3::Length(GLuint div_point_count) const
    {
        GLdouble step = (_u_max - _u_min) / (2 * div_point_count);

        LinearCombination3::Derivatives d;

        GLdouble integral = 0.0;

        GLdouble even = 0.0;
        for (GLuint j = 1; j <= div_point_count - 1; ++j)
        {
            GLdouble u = _u_min + 2 * j * step;

            bspc->CalculateDerivatives(1, u, d);

            even += d[1].length();

        }
        even *= 2.0;
        integral += even;

        GLdouble odd = 0.0;
        for (GLuint j = 1; j <= div_point_count; ++j)
        {
            GLdouble u = _u_min + (2 * j - 1) * step;

            d.LoadNullVectors();

            bspc->CalculateDerivatives(1, u, d);

            odd += d[1].length();
        }
        odd *= 4.0;

        integral += odd;

        bspc->CalculateDerivatives(1, _u_min, d);

        integral += d[1].length();

        bspc->CalculateDerivatives(1, _u_max, d);

        integral += d[1].length();

        integral *= step;
        integral /= 3.0;

        return integral;
    }

    RowMatrix<GLdouble> ParametricCurve3::prodRowMatrix(RowMatrix<GLdouble> a, RowMatrix<GLdouble> b)
    {
       if (a.GetColumnCount() != b.GetColumnCount())
       {
           throw Exception("Matrix dimensions must agree");
       }

       RowMatrix<GLdouble> result;
       result.ResizeColumns(a.GetColumnCount());
       for (GLint i = 0; i < a.GetColumnCount(); i++)
       {
           result[i] = a[i] * b[i];
       }

       return result;

    }

    RowMatrix<GLdouble> ParametricCurve3::f(RowMatrix<GLdouble> v, GLuint n)
    {
        RowMatrix<GLdouble> result;
        for (GLuint i=0; i<n; i++)
        {
            for (GLuint j=0; j<n-i;j++)
            {
                // f
            }
        }

        return result;
    }

    RowMatrix<GLdouble> ParametricCurve3::A(RowMatrix<GLdouble> u, GLint n, GLint i){
        unsigned long long combination = nchoosek(n,i);

        // u.^i
        RowMatrix<GLdouble> u_i;
        u_i.ResizeColumns(n);
        for (GLuint j=0;j<n;j++)
        {
            u_i[j] = pow(u[j], i);
        }

        // (1-u).^(n-i)
        RowMatrix<GLdouble> one_u_pow_n_i;
        one_u_pow_n_i.ResizeColumns(n);
        for (GLuint j=0;j<n;j++)
        {
            one_u_pow_n_i = pow(1-u[j], n-i);
        }

        RowMatrix<GLdouble> result;
        result.ResizeColumns(n);
        for (GLuint j=0;j<n;j++)
        {
            result = combination * u_i[j] * one_u_pow_n_i[j];
        }

        return result;
    }

//    % reparametrized Bernstein polynomials
//    TA = @(u,n,i)A((u-u_min)/(u_max - u_min), n, i);
    RowMatrix<GLdouble> ParametricCurve3::TA(RowMatrix<GLdouble> u, GLint n, GLint i){
       RowMatrix<GLdouble> u_umin;

       for (GLuint j = 0; j<n;j++)
       {
           u_umin[j]=(u[j]-_u_min)/(_u_max-_u_min);

       }

        return A(u_umin, n, i);
    }

    unsigned long long
            ParametricCurve3::nchoosek(unsigned long long n, unsigned long long k) {
        if (k > n) {
            return 0;
        }
        unsigned long long r = 1;
        for (unsigned long long d = 1; d <= k; ++d) {
            r *= n--;
            r /= d;
        }
        return r;
    }

    RowMatrix<GLdouble> ParametricCurve3::linspace(GLdouble min, GLdouble max, int n)
    {
        RowMatrix<GLdouble> result;
        result.ResizeColumns(n);

        GLdouble step = (max - min) / n;

        for (int i = 0; i < n; i++)
        {
            result[i] = min + i * step;
        }

        return result;
    }
}
